/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: IMUServer.cpp,v 1.4 2009-05-13 07:18:16 amaula Exp $
 *
 * \file
 * \brief Implementation of IMU interface Server.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */

#include "IMUServer.hpp"
#include "binbag.h"

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
using namespace MaCI::IMU;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
CIMUServer::CIMUServer(MaCICtrl::CMaCICtrlServer *aMaCICtrlServerPtr, 
                       const int aInterfaceMinor,
                       const std::string &aDescription)
  :CMaCI(aMaCICtrlServerPtr, GIMI_PROTOCOL_MACI_IMU, aInterfaceMinor),
   CThread(1),
   CSync(1,1),
   iServiceDescription(aDescription),
   iServerActive(false),
   iEventTypes(),
   iEventTypesSet(false),
   iIMUDataArray(),
   iReplyDataArray()
{
}
//*****************************************************************************
     
CIMUServer::~CIMUServer()
{
  DoClose();
}
//*****************************************************************************
      
MaCI::EMaCIError CIMUServer::DoOpen(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);
  
  // Execute only if server is not active.
  if (!iServerActive && iEventTypesSet) {
    // Register service.
    RegisterProvidedService("MaCI-IMUServer '" + iServiceDescription + "'");
    RegisterAcceptedService("MaCI-IMUServer-Cmd; '" + iServiceDescription + "'");

    // Start the handler Thread
    iServerActive = true;
    SetActive(true);
    CThread::RunThread();
    result = KMaCIOK;
    
    dPrint(4,"Succesfully Open():ed IMUServer instance %p.", this);
  }else{
    if(iServerActive){
      dPrint(3,"IMU Server already active");
    }
    if(!iEventTypesSet){
      dPrint(3,"Event types hasn't been set! Can't open the Server");
    }
  }
    
  return result;
  

}
//*****************************************************************************

      
MaCI::EMaCIError CIMUServer::DoClose(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);

  // Execute block only if server is currently active.
  if (iServerActive) {
    // Unregister services
    UnregisterProvidedService();
    UnregisterAcceptedService();
    
    // Stop the handler Thread
    iServerActive = false;
    SetActive(false);
    /* CThread::CancelThread(); //Removed by script */
    CThread::WaitThread();

    dPrint(4,"Succesfully Close():ed IMUServer instance %p", this);
  }
  
  return result;
  
}
//*****************************************************************************
      
MaCI::EMaCIError CIMUServer::WaitCommand(CIMUData &aData, 
                                         TReplyInfo &aInfo,
                                         const unsigned int aTimeout_ms,
                                         bool aSkipQueued)
{
  EMaCIError result = KMaCIError;
  const unsigned int time_begin = ownTime_get_ms();
  int timeout = aTimeout_ms;
  
  aData.Reset();
  Lock();
  do {
    const unsigned int asize = iIMUDataArray.size();
    // Check whether there are commands in buffer.
    if (asize > 0) {
      // Got element. So; take reference of its pointer, copy it to
      // 'data', delete it. Pop the element from queue.
      CIMUData *d;
      // Check whether we want to skip queue 
      if (aSkipQueued && asize > 1) {
        // This is only executed if there is >1 in queue.  Otherwise
        // default branch is executed.  Want to skip, so get back()
        // reference.
        d= iIMUDataArray.back();
        // Copy data
        aData = *d;
        aInfo = iReplyDataArray.back();

        // Delete all elements.
        for(EACH_IN_i(iIMUDataArray)) {
          delete *i;
        }

        // Clear array.
        iIMUDataArray.clear();
        iReplyDataArray.clear();

        
        // print debug
        dPrint(4,"Flushed %d unprocessed commands out of buffer",
               asize-1);

      } else {
        // No skipping, so get front() reference;
        d = iIMUDataArray.front();
        // Copy data.
        aData = *d;
        aInfo = iReplyDataArray.front();
        // Delete source
        delete d;
        // pop the entry from queue.
        iIMUDataArray.pop_front();
        iReplyDataArray.pop_front();
        
      }

      // Set result.
      result = KMaCIOK;
      
      // Break the loop, skipping Wait()
      break;
    }
    
    // Wait signal unconditionally.
    if (timeout > 0) Wait(timeout);
    
    // Recalculate timeout
    timeout = aTimeout_ms - ownTime_get_ms_since(time_begin);
  } while(timeout > 0 && iServerActive);
  Unlock();

  // Change result type if timeout was the reason.
  if (timeout <= 0) result = KMaCITimeout;


  return result;

}
//*****************************************************************************

bool CIMUServer::SendEvent(const CIMUData &aData)
{
  bool result = false;

  assert(iGIMIPtr != NULL);
  const TCommand *com = aData.GetCommand();
  if(com){
    if(com->cmd == KCommandEvent){
   
      EMaCIError e = 
        SendInterfaceSubscriberMessage(aData.GetBinBagContainerPtr()->GetBuffer(),
                                       aData.GetBinBagContainerPtr()->GetBufferSize());
      result = (e == KMaCIOK);
    }else{
      dPrint(4,"Wrong command set, should be KCommandEvent");
    }

  } else {
    dPrint(4,"No Command set in container, will not send!");
 
  }

  return result;
  
}
//*****************************************************************************

MaCI::EMaCIError CIMUServer::SendReply(CIMUData &aData,
                                       TReplyInfo &aInfo)
{
  aData.Print(10);
  EMaCIError result = KMaCIError;
  if(iServerActive){
    gim::binbag::CBinBag replybb;
    // Do encoding to this binbag
    //    aData.EncodeTo(&replybb);
    
    int r = iGIMIPtr->send(aInfo.senderId,
			   aData.GetBinBagContainerPtr()->GetBuffer(),
			   aData.GetBinBagContainerPtr()->GetBufferSize(),
			   iInterfaceMajor,
			   iInterfaceMinor,
			   aInfo.msgId);
    
    if (r == GIMI_OK) {
      dPrint(8,"Succesfully sent message with %d BinBag elements to '%p'",
	     replybb.GetElementCount(),
	     aInfo.senderId);

      result = KMaCIOK;
      
    } else {
      
      dPrint(5,"Failed to send message with %d BinBag elements to '%p'",
	     replybb.GetElementCount(),
	     aInfo.senderId);
    }
  }else{
    result = KMaCINoConnection;
    
  }

  return result;
}
//*****************************************************************************

bool CIMUServer::SetEventTypes(TEventTypes &aType)
{
  iEventTypes = aType;
  //dPrint(1,"eventtype setted to %d",iEventTypes.continuousTypes);
  iEventTypesSet = true;
  return false;
}
//*****************************************************************************
int CIMUServer::ThreadFunction(const int aThreadNumber)
{

 int result = 0;
  EMaCIError e;
  gimi::GIMIMessage gmsg;
  
  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);
  
  dPrint(4, "GIMIMessage handler started");
  
  // Run as long as the thread should be active
  while(iServerActive) {
    // Call the ReceiveMessage wrapper with 1s timeout
    e = ReceiveInterfaceMessage(gmsg, 1000);
    if (e == KMaCIOK) {
      dPrint(10,"Got GIMIMessage %d - Processing.", gmsg.messageId);
      ProcessMessage(gmsg);
      
    } else if (e != KMaCITimeout) {
      // Other error, try to recover by sleeping one second.
      ownSleep_ms(1000);
      
    }
  }
  
  dPrint(4, "GIMIMessage handler stopped");

  return result;
 
}
//*****************************************************************************
int CIMUServer::ProcessMessage(const gimi::GIMIMessage &msg)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CIMUData d;
  bool r;

  r = bb.Decode(msg.getData(), msg.getDatalength());
  if (r) {
    // Succesfully decoded as BinBag.
    r = d.DecodeFrom(&bb);
    if (r) {

      // Succesfully decoded as BehaviourData
      // Now, Check whether it was Command, if so, process it and construct reply
      gim::binbag::CBinBag replybb;
      CIMUData reply;
      bool send_reply = false;

      // Do encoding to this binbag
      reply.EncodeTo(&replybb);
      const TCommand *cmd = d.GetCommand();
      if (cmd) {
        switch(cmd->cmd) {
        
        case KCommandGetEventTypes:{
          dPrint(1,"Asked for event types. replying %u",iEventTypes.continuousTypes);
          reply.SetCommand(TCommand(KCommandGetEventTypesReply));
          reply.SetEventTypes(iEventTypes);
          send_reply = true;
          break;
        }

        default:{
          // Debug
          dPrint(8, "Got Command to be passed to User (Command was %d)",
                 cmd->cmd);
          
          // Save the data
          CIMUData *new_entry = new CIMUData(d);
          
          Lock();
          iIMUDataArray.push_back(new_entry);
          iReplyDataArray.push_back(TReplyInfo(msg.getSenderId(),msg.messageId));
          Notify();
          Unlock();
          break;

        }
        }
      }
          
      
      // Check whether reply needs to be sent
      if (send_reply) {
        
	int gr = iGIMIPtr->send(msg.getSenderId(),
				replybb.GetBuffer(),
				replybb.GetBufferSize(),
				iInterfaceMajor,
				iInterfaceMinor,
				msg.messageId); // Reply with same ID
	if (gr == GIMI_OK) {
	  dPrint(1,"Succesfully sent message with %d BinBag elements to '%p'",
		 replybb.GetElementCount(),
		 msg.getSenderId());
          
	} else {
	  
	  dPrint(1,"Failed to send message with %d BinBag elements to '%p'",
		 replybb.GetElementCount(),
		 msg.getSenderId());
	}
      }
    }else{
      dPrint(3,"DecodeFrom FAILED!");
    }
  }
  

  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
